//--------------------------- Constants ----------------------------------
import BaseEntity from "./BaseEntity";
import Obstacle from "./Obstacle";
import Path from "./Path";
import Vehicle from "./Vehicle";
// import Wall from "./Wall";

//躲避圆半径
const WanderRad: number = 1.2;
//在智能体躲避行为中前面圆的距离
const WanderDist: number = 2;
//躲避最大角度
const WanderJitterPerSec: number = 80.0;
//路径根据距离阀值
const WaypointSeekDist: number = 20;


enum SummingMethod { weighted_average, prioritized, dithered }
let offset = 0;
enum BehaviorType {
    none = 1 << offset++,
    seek = 1 << offset++,
    flee = 1 << offset++,
    arrive = 1 << offset++,
    wander = 1 << offset++,
    cohesion = 1 << offset++,
    separation = 1 << offset++,
    allignment = 1 << offset++,
    obstacleAvoidance = 1 << offset++,
    wallAvoidance = 1 << offset++,
    followPath = 1 << offset++,
    pursuit = 1 << offset++,
    evade = 1 << offset++,
    interpose = 1 << offset++,
    hide = 1 << offset++,
    flock = 1 << offset++,
    offsetPursuit = 1 << offset++,
    flow = 1 << offset++
}

enum Deceleration {
    SLOW = 3,
    NORMAL = 2,
    FAST = 1,
}

let tempVec2 = cc.v2();
let tempVec2_2 = cc.v2();
let tempVec2_3 = cc.v2();
let tempVec2_4 = cc.v2();
let tempVec2_5 = cc.v2();
let tempVec2_6 = cc.v2();
let tempMat4 = cc.mat4();
export default class SteeringBehavior {
    private _vehicle: Vehicle = null;
    private _steeringForce: cc.Vec2 = cc.Vec2.ZERO;
    private _targetAgent1: Vehicle = null;
    private _targetAgent2: Vehicle = null;

    private static _steeringForceTweaker: number = 200;
    //权重,使用权重比例计算操控力的才需要
    //////////////////////////////////////////////////////////
    private _weightSeparation: number = 2 * SteeringBehavior._steeringForceTweaker;
    private _weightCohesion: number = 0.5 * SteeringBehavior._steeringForceTweaker;
    private _weightAlignment: number = 1 * SteeringBehavior._steeringForceTweaker;
    private _weightObstacleAvoidance: number = 10 * SteeringBehavior._steeringForceTweaker;
    private _weightWallAvoidance: number = 10 * SteeringBehavior._steeringForceTweaker;
    private _weightSeek: number = 1 * SteeringBehavior._steeringForceTweaker;
    private _weightFlee: number = 1 * SteeringBehavior._steeringForceTweaker;
    private _weightArrive: number = 1 * SteeringBehavior._steeringForceTweaker;
    private _weightPursuit: number = 1 * SteeringBehavior._steeringForceTweaker;
    private _weightOffsetPursuit: number = 1 * SteeringBehavior._steeringForceTweaker;
    private _weightInterpose: number = 1 * SteeringBehavior._steeringForceTweaker;
    private _weightHide: number = 1 * SteeringBehavior._steeringForceTweaker;
    private _weightFollowPath: number = 0.05 * SteeringBehavior._steeringForceTweaker;
    private _weightEvade: number = 0.01 * SteeringBehavior._steeringForceTweaker;
    private _weightWander: number = 1 * SteeringBehavior._steeringForceTweaker;
    private _weightFlow: number = 3 * SteeringBehavior._steeringForceTweaker;
    //////////////////////////////////////////////////////////

    private _curTarget: cc.Vec2 = cc.Vec2.ZERO;

    private _boxLength: number = 40;//检测障碍物的探测长度


    private _feelers: cc.Vec2[] = [];
    public get feelers(): cc.Vec2[] {
        return this._feelers;
    }

    public debugPen: cc.Graphics = null;

    private _wallDetectionFeelerLength: number = 40;

    private _wanderTarget: cc.Vec2 = cc.Vec2.ZERO;
    public get wanderTarget() {
        return this._wanderTarget;
    }
    private _wanderJitter: number = WanderJitterPerSec;
    private _wanderRadius: number = WanderRad;
    private _wanderDistance: number = WanderDist;

    //100半径内可视，用于视野内集群行为力
    private _viewDistance: number = 50;

    private _wayPointSeekDistSq: number = WaypointSeekDist * WaypointSeekDist;

    private _offset: cc.Vec2 = cc.Vec2.ZERO;

    private _flags: number = 0;

    private _deceleration: Deceleration = Deceleration.FAST;

    //标记是否开启空间划分
    private _cellSpaceOn: boolean = false;

    //合力计算方式 ，默认优先加权截断
    private _summingMethod: SummingMethod = SummingMethod.prioritized;

    private on(bt: BehaviorType): boolean { return (this._flags & bt) == bt; }

    //跟随路径
    private _path: Path = null;

    //按照优先级计算操控力
    private accumulateForce(forceToAdd: cc.Vec2): boolean {
        let mag = this._steeringForce.mag();
        let remain = this._vehicle.maxForce - mag;//剩余多少力
        if (remain <= 0.0) return false;
        let toadd = forceToAdd.mag();
        if (toadd < remain) {
            this._steeringForce.addSelf(forceToAdd);
        } else {
            this._steeringForce.addSelf(forceToAdd.normalizeSelf().mulSelf(remain));
        }
        return true;
    }

    /* .......................................................

                    BEGIN BEHAVIOR DECLARATIONS

      .......................................................*/
    //靠近目标
    private seek(targetPos: cc.Vec2): cc.Vec2 {
        this._seekVec.set(targetPos);
        cc.Vec2.subtract(tempVec2, targetPos, this._vehicle.position);
        cc.Vec2.normalize(tempVec2, tempVec2);
        cc.Vec2.multiplyScalar(tempVec2, tempVec2, this._vehicle.maxSpeed);
        cc.Vec2.subtract(tempVec2, tempVec2, this._vehicle.velocity);
        return tempVec2;
    }
    //远离目标
    private flee(targetPos: cc.Vec2): cc.Vec2 {
        cc.Vec2.subtract(tempVec2, this._vehicle.position, targetPos);
        if (tempVec2.magSqr() > 90000) {//可设定特定距离才远离
            cc.Vec2.set(tempVec2, 0, 0);
            return tempVec2;
        }
        cc.Vec2.normalize(tempVec2, tempVec2);
        cc.Vec2.multiplyScalar(tempVec2, tempVec2, this._vehicle.maxSpeed);
        cc.Vec2.subtract(tempVec2, tempVec2, this._vehicle.velocity);
        return tempVec2;
    }

    //到达某个点
    private arrive(targetPos: cc.Vec2, deceleration: Deceleration): cc.Vec2 {
        cc.Vec2.subtract(tempVec2, targetPos, this._vehicle.position);
        let dis = tempVec2.mag();
        if (dis > 0.01) {
            let speed = dis / (deceleration * 0.3);
            speed = Math.min(speed, this._vehicle.maxSpeed);
            cc.Vec2.multiplyScalar(tempVec2, tempVec2, speed);
            tempVec2.divSelf(dis);
            cc.Vec2.subtract(tempVec2, tempVec2, this._vehicle.velocity);
            return tempVec2;
        }
        cc.Vec2.set(tempVec2, 0, 0);
        return tempVec2;
    }

    private _seekVec:cc.Vec2 = cc.v2();
    //追逐
    private pursuit(agent: Vehicle): cc.Vec2 {
        if (!agent) {
            tempVec2.set(cc.Vec2.ZERO);
            return tempVec2;
        }
        cc.Vec2.subtract(tempVec2, agent.position, this._vehicle.position);
        let relativeHeading = this._vehicle.heading.dot(agent.heading);
        if (tempVec2.dot(this._vehicle.heading) > 0 && relativeHeading < -0.95) {
            return this.seek(agent.position);
        }
        let lookaheadTime = tempVec2.mag() / (this._vehicle.maxSpeed + agent.velocity.mag());
        cc.Vec2.multiplyScalar(tempVec2_2, agent.velocity, lookaheadTime);
        cc.Vec2.add(tempVec2_2, tempVec2_2, agent.position);
        return this.seek(tempVec2_2);
    }

    private offsetPursuit(agent: Vehicle, offset: cc.Vec2): cc.Vec2 {
        if (!agent) {
            tempVec2.set(cc.Vec2.ZERO);
            return tempVec2;
        }
        cc.Vec2.set(tempVec2, agent.side.x * offset.x + agent.side.y * offset.y, agent.heading.x * offset.x + agent.heading.y * offset.y);
        cc.Vec2.add(tempVec2, agent.position, tempVec2);
        let dis = cc.Vec2.distance(tempVec2, this._vehicle.position);
        let lookAheadTime = dis / (this._vehicle.maxSpeed + 0);
        cc.Vec2.multiplyScalar(tempVec2_2, agent.velocity, lookAheadTime);
        cc.Vec2.add(tempVec2_2, tempVec2, tempVec2_2);
        return this.arrive(tempVec2_2, this._deceleration);
    }

    //躲避
    private evade(agent: Vehicle): cc.Vec2 {
        cc.Vec2.subtract(tempVec2, agent.position, this._vehicle.position);
        let lensqr = tempVec2.magSqr();
        if (lensqr > 40000) { //可设定特定距离之后不躲避
            cc.Vec2.set(tempVec2, 0, 0);
            return tempVec2;
        } else {
            let lookAheadTime = tempVec2.mag() / (this._vehicle.maxSpeed + agent.velocity.mag());
            cc.Vec2.multiplyScalar(tempVec2, agent.velocity, lookAheadTime);
            cc.Vec2.add(tempVec2_2, tempVec2, agent.position);
            return this.flee(tempVec2_2);
        }
    }

    private randomClamped() {
        return Math.random() - Math.random();
    }
    //徘徊
    private wander(): cc.Vec2 {
        let jitterTimeScale = this._wanderJitter * this._vehicle.timeElapse();
        cc.Vec2.set(tempVec2_2, this.randomClamped() * jitterTimeScale, this.randomClamped() * jitterTimeScale);
        cc.Vec2.add(this._wanderTarget, this._wanderTarget, tempVec2_2);
        this._wanderTarget.normalizeSelf();
        cc.Vec2.multiplyScalar(this._wanderTarget, this._wanderTarget, this._wanderRadius);
        cc.Vec2.set(tempVec2_2, 0, this._wanderDistance);
        cc.Vec2.add(tempVec2, this._wanderTarget, tempVec2_2);
        this._vehicle.node.getLocalMatrix(tempMat4);
        cc.Vec2.transformMat4(tempVec2, tempVec2, tempMat4);
        cc.Vec2.subtract(tempVec2, tempVec2, this._vehicle.position);
        return tempVec2;
    }

    //躲避障碍物
    private obstacleAvoidance(obstacles: Obstacle[]): cc.Vec2 {
        this._boxLength = 40 + (this._vehicle.velocity.mag() / this._vehicle.maxSpeed) * 40;
        let distToClose = Number.MAX_SAFE_INTEGER;
        let closeIndex = -1;
        cc.Vec2.set(tempVec2_3, 0, 0);
        this._vehicle.node.getLocalMatrix(tempMat4);
        cc.Mat4.invert(tempMat4, tempMat4);
        for (let i = 0, len = obstacles.length; i < len; i++) {
            let obstacle = obstacles[i];
            cc.Vec2.set(tempVec2_2, obstacle.node.x, obstacle.node.y);
            cc.Vec2.transformMat4(tempVec2_2, tempVec2_2, tempMat4);
            if (tempVec2_2.y >= 0 && tempVec2_2.y <= this._boxLength + obstacle.radius) {
                //圆方程 (x-a)^2+(y-b)^2=r^2  a,b圆心坐标 r半径
                //y = b+-sqrt(r^2-a^2)
                let expandeRadius = obstacle.radius + this._vehicle.radius;
                if (Math.abs(tempVec2_2.x) < expandeRadius) {
                    let sqrtpart = Math.sqrt(expandeRadius * expandeRadius - tempVec2_2.x * tempVec2_2.x);
                    let ip = tempVec2_2.y - sqrtpart;
                    if (ip <= 0) {
                        ip = tempVec2_2.y + sqrtpart;
                    }
                    if (ip < distToClose) {
                        distToClose = ip;
                        closeIndex = i;
                        cc.Vec2.set(tempVec2_3, tempVec2_2.x, tempVec2_2.y);
                    }
                }
            }
        }

        if (closeIndex >= 0) {
            let closeobstacle = obstacles[closeIndex];
            let multiplier = 1.0 + (this._boxLength - tempVec2_3.y) / this._boxLength;
            tempVec2_2.x = (closeobstacle.radius - tempVec2_3.x) * multiplier;
            tempVec2_2.y = (closeobstacle.radius - tempVec2_3.y) * 0.2;
            this._vehicle.node.getLocalMatrix(tempMat4);
            cc.Vec2.transformMat4(tempVec2, tempVec2_2, tempMat4);
            return tempVec2;
        } else {
            tempVec2_2.set(cc.Vec2.ZERO);
            return tempVec2_2;
        }
    }

    private _createFeelers() {
        //中间触角
        cc.Vec2.multiplyScalar(tempVec2, this._vehicle.heading, 1.2 * this._vehicle.radius);
        cc.Vec2.add(tempVec2, this._vehicle.position, tempVec2);
        if (this._feelers.length <= 0) {
            this._feelers.push(cc.v2(tempVec2));
        } else {
            this._feelers[0].set(tempVec2);
        }
        //左边触角
        this._vehicle.heading.rotate(cc.misc.degreesToRadians(60), tempVec2);
        cc.Vec2.multiplyScalar(tempVec2, tempVec2, this._vehicle.radius);
        cc.Vec2.add(tempVec2, tempVec2, this._vehicle.position);
        if (this._feelers.length <= 1) {
            this._feelers.push(cc.v2(tempVec2));
        } else {
            this._feelers[1].set(tempVec2);
        }
        //右边触角
        this._vehicle.heading.rotate(cc.misc.degreesToRadians(-60), tempVec2);
        cc.Vec2.multiplyScalar(tempVec2, tempVec2, this._vehicle.radius);
        cc.Vec2.add(tempVec2, tempVec2, this._vehicle.position);
        if (this._feelers.length <= 2) {
            this._feelers.push(cc.v2(tempVec2));
        } else {
            this._feelers[2].set(tempVec2);
        }
    }
    //躲避墙
    // private wallAvoidance(walls: Wall[]): cc.Vec2 {
    //     this._createFeelers();
    //     let closeWall = -1;
    //     let distClose = 1 << 30;
    //     for (let i = 0, len = this._feelers.length; i < len; i++) {
    //         for (let j = 0, jlen = walls.length; j < jlen; j++) {
    //             let info = this._lineIntersection2D(this._vehicle.position, this._feelers[i], walls[j].from, walls[j].to, tempVec2_4);
    //             if (info[0]) {
    //                 if (info[1] < distClose) {
    //                     distClose = info[1];
    //                     closeWall = j;
    //                     tempVec2_5.set(tempVec2_4);
    //                 }
    //             }
    //         }
    //         if (closeWall >= 0) {
    //             cc.Vec2.subtract(tempVec2_5, this._feelers[i], tempVec2_5);
    //             cc.Vec2.multiplyScalar(tempVec2, walls[closeWall].normal, tempVec2_5.mag() * 2);
    //         }
    //     }
    //     if (closeWall == -1) {
    //         tempVec2.set(cc.Vec2.ZERO);
    //     }
    //     return tempVec2;
    // }

    //路径跟随
    private followPath(): cc.Vec2 {
        let waypoint = this._path.getCurPoint();
        if (!waypoint) return tempVec2.set(cc.Vec2.ZERO);
        if (cc.Vec2.squaredDistance(waypoint, this._vehicle.position) < this._wayPointSeekDistSq) {
            this._path.setUpNextPoint();
        }
        if (!this._path.isFinish()) {
            return this.seek(waypoint);
        } else {
            return this.arrive(waypoint, Deceleration.NORMAL);
        }
    }
    private interpose(vehicleA: Vehicle, vehicleB: Vehicle): cc.Vec2 {
        cc.Vec2.add(tempVec2, vehicleA.position, vehicleB.position);
        cc.Vec2.multiplyScalar(tempVec2, tempVec2, 0.5);
        let timeToReachMidPoint = cc.Vec2.distance(this._vehicle.position, tempVec2) / this._vehicle.maxSpeed;
        cc.Vec2.multiplyScalar(tempVec2, vehicleA.velocity, timeToReachMidPoint);
        cc.Vec2.add(tempVec2, tempVec2, vehicleA.position);
        cc.Vec2.multiplyScalar(tempVec2_2, vehicleB.velocity, timeToReachMidPoint);
        cc.Vec2.add(tempVec2_2, tempVec2_2, vehicleB.position);
        cc.Vec2.add(tempVec2, tempVec2, tempVec2_2);
        cc.Vec2.multiplyScalar(tempVec2_2, tempVec2, 0.5);

        return this.arrive(tempVec2_2, Deceleration.FAST);
    }

    private hide(hunter: Vehicle, obstacles: Array<BaseEntity>): cc.Vec2 {
        let distToClosest = 999999999;
        let closeIndex = -1;
        cc.Vec2.set(tempVec2_3, 0, 0);
        for (let i = 0, len = obstacles.length; i < len; i++) {
            let obstacle = obstacles[i];
            let vec = this._getHidingPosition(obstacle.position, obstacle.radius, hunter.position);
            let dist = cc.Vec2.squaredDistance(vec, this._vehicle.position);
            if (dist < distToClosest) {
                distToClosest = dist;
                cc.Vec2.set(tempVec2_3, vec.x, vec.y);
                closeIndex = i;
            }
        }
        if (closeIndex == -1) {
            return this.evade(hunter);
        } else {
            return this.arrive(tempVec2_3, Deceleration.FAST);
        }
    }

    private _getHidingPosition(posOb: cc.Vec2, radiusOb: number, posHunter: cc.Vec2) {
        let disFromBoundary = 30;
        let disAway = radiusOb + disFromBoundary;
        cc.Vec2.subtract(tempVec2, posOb, posHunter);
        tempVec2.normalizeSelf();
        cc.Vec2.multiplyScalar(tempVec2, tempVec2, disAway);
        cc.Vec2.add(tempVec2_2, tempVec2, posHunter);
        return tempVec2_2;
    }

    private flow() {
        let pos2 = this._vehicle.world.getColAndRowByPosition(this._vehicle.position.x, this._vehicle.position.y);
        if (this._vehicle.world.isVaildGrid(pos2[0], pos2[1])) {
            
            //TODO
            let gridcell = this._vehicle.world.gridCells[pos2[0]][pos2[1]];
            let offsetx = 0, offsety = 0, dirx = 0, diry = 0;
            let pos = this._vehicle.world.getPixelCenterInTilePos(tempVec2_3.x, tempVec2_3.y);
            if (Math.abs(gridcell.flowDir.x) <= 0.02) {
                diry = gridcell.flowDir.y > 0 ? 1 : -1;
                dirx = pos[0] > this._vehicle.position.x ? -1 : 1;
                if (!this._vehicle.world.isVaildGrid(tempVec2_3.x + dirx, tempVec2_3.y + diry)) {
                    offsetx = -0.25 * dirx;
                }
            } else if (Math.abs(gridcell.flowDir.y) <= 0.02) {
                dirx = gridcell.flowDir.x > 0 ? 1 : -1;
                diry = pos[1] > this._vehicle.position.y ? -1 : 1;
                if (!this._vehicle.world.isVaildGrid(tempVec2_3.x + dirx, tempVec2_3.y + diry)) {
                    offsety = 0.25 * diry;
                }
            }
            cc.Vec2.set(tempVec2, gridcell.flowDir.x + offsetx, -(gridcell.flowDir.y + offsety));
        } else {
            tempVec2.set(cc.Vec2.ZERO);
        }
        return tempVec2;
    }

    // -- Group Behaviors -- //
    private cohesion(agents: Array<Vehicle>): cc.Vec2 {
        cc.Vec2.set(tempVec2_6, 0, 0);
        cc.Vec2.set(tempVec2_5, 0, 0);
        let neighborcount = 0;
        for (let i = 0, len = agents.length; i < len; i++) {
            let agent = agents[i];
            if (agent != this._vehicle && agent != this._targetAgent1 && agent.isTag) {
                cc.Vec2.add(tempVec2_6, tempVec2_6, agent.position);
                neighborcount++;
            }
        }
        if (neighborcount > 0) {
            tempVec2_6.divSelf(neighborcount);
            let tempv2 = this.seek(tempVec2_6);
            cc.Vec2.set(tempVec2_5, tempv2.x, tempv2.y);
            tempVec2_5.normalizeSelf();
            return tempVec2_5;
        }
        return cc.Vec2.ZERO;
    }
    private separation(agents: Array<Vehicle>): cc.Vec2 {
        cc.Vec2.set(tempVec2, 0, 0);
        for (let i = 0, len = agents.length; i < len; i++) {
            if (agents[i].ID != this._vehicle.ID && agents[i].isTag && agents[i] != this._targetAgent1) {
                cc.Vec2.subtract(tempVec2_2, this._vehicle.position, agents[i].position);
                let len = tempVec2_2.mag();
                cc.Vec2.normalize(tempVec2_2, tempVec2_2);
                tempVec2_2.divSelf(len);
                cc.Vec2.add(tempVec2, tempVec2_2, tempVec2);
            }
        }
        return tempVec2;
    }
    private alignment(agents: Array<Vehicle>): cc.Vec2 {
        cc.Vec2.set(tempVec2_6, 0, 0);
        cc.Vec2.set(tempVec2_5, 0, 0);
        let neighborcount = 0;
        for (let i = 0, len = agents.length; i < len; i++) {
            if (agents[i].ID != this._vehicle.ID && agents[i].isTag && agents[i] != this._targetAgent1) {
                cc.Vec2.add(tempVec2_6, tempVec2_6, agents[i].heading);
                neighborcount++;
            }
        }
        if (neighborcount > 0) {
            tempVec2_6.divSelf(neighborcount);
            cc.Vec2.subtract(tempVec2_5, tempVec2_6, this._vehicle.heading);
        }
        return tempVec2_5;
    }

    private cohesionPlus(agents: Array<Vehicle>): cc.Vec2 { return cc.Vec2.ZERO }
    private separationPlus(agents: Array<Vehicle>): cc.Vec2 { return cc.Vec2.ZERO }
    private alignmentPlus(agents: Array<Vehicle>): cc.Vec2 { return cc.Vec2.ZERO }

    /* .......................................................

                   END BEHAVIOR DECLARATIONS

  .......................................................*/

    private calculateWeightedSum(): cc.Vec2 { return cc.Vec2.ZERO }
    private calculatePrioritized(): cc.Vec2 {
        let force: cc.Vec2;
        // if (this.on(BehaviorType.wallAvoidance)) {
        //     force = this.wallAvoidance(this._vehicle.world.walls);
        //     cc.Vec2.multiplyScalar(force, force, this._weightWallAvoidance);
        //     if (!this.accumulateForce(force)) return this._steeringForce;
        // }
        if (this.on(BehaviorType.obstacleAvoidance)) {
            force = this.obstacleAvoidance(this._vehicle.world.obstacles);
            cc.Vec2.multiplyScalar(force, force, this._weightObstacleAvoidance);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }
        if (this.on(BehaviorType.flow)) {
            force = this.flow();
            cc.Vec2.multiplyScalar(force, force, this._weightFlow);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }
        if (this.on(BehaviorType.evade)) {//躲避
            force = this.evade(this._targetAgent1);
            cc.Vec2.multiplyScalar(force, force, this._weightEvade);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }
        if (this.on(BehaviorType.flee)) {//远离
            force = this.flee(this._curTarget);
            cc.Vec2.multiplyScalar(force, force, this._weightFlee);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }

        //组合行为
        if (!this.isSpacePartitioningOn()) {
            if (this.on(BehaviorType.separation)) {
                force = this.separation(this._vehicle.world.agents);
                cc.Vec2.multiplyScalar(force, force, this._weightSeparation);
                if (!this.accumulateForce(force)) return this._steeringForce;
            }
            if (this.on(BehaviorType.allignment)) {
                force = this.alignment(this._vehicle.world.agents);
                cc.Vec2.multiplyScalar(force, force, this._weightAlignment);
                if (!this.accumulateForce(force)) return this._steeringForce;
            }
            if (this.on(BehaviorType.cohesion)) {
                force = this.cohesion(this._vehicle.world.agents);
                cc.Vec2.multiplyScalar(force, force, this._weightCohesion);
                if (!this.accumulateForce(force)) return this._steeringForce;
            }
        } else {
            if (this.on(BehaviorType.separation)) {
                force = this.separationPlus(this._vehicle.world.agents);
                cc.Vec2.multiplyScalar(force, force, this._weightSeparation);
                if (!this.accumulateForce(force)) return this._steeringForce;
            }
            if (this.on(BehaviorType.allignment)) {
                force = this.alignmentPlus(this._vehicle.world.agents);
                cc.Vec2.multiplyScalar(force, force, this._weightAlignment);
                if (!this.accumulateForce(force)) return this._steeringForce;
            }
            if (this.on(BehaviorType.cohesion)) {
                force = this.cohesionPlus(this._vehicle.world.agents);
                cc.Vec2.multiplyScalar(force, force, this._weightCohesion);
                if (!this.accumulateForce(force)) return this._steeringForce;
            }
        }

        if (this.on(BehaviorType.seek)) {
            force = this.seek(this._curTarget);
            cc.Vec2.multiplyScalar(force, force, this._weightSeek);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }

        if (this.on(BehaviorType.arrive)) {
            force = this.arrive(this._curTarget, this._deceleration);
            cc.Vec2.multiplyScalar(force, force, this._weightArrive);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }

        if (this.on(BehaviorType.wander)) {
            force = this.wander();
            cc.Vec2.multiplyScalar(force, force, this._weightWander);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }

        if (this.on(BehaviorType.pursuit)) {
            force = this.pursuit(this._targetAgent1);
            cc.Vec2.multiplyScalar(force, force, this._weightPursuit);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }

        if (this.on(BehaviorType.offsetPursuit)) {
            force = this.offsetPursuit(this._targetAgent1, this._offset);
            cc.Vec2.multiplyScalar(force, force, this._weightOffsetPursuit);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }

        if (this.on(BehaviorType.interpose)) {
            force = this.interpose(this._targetAgent1, this._targetAgent2);
            cc.Vec2.multiplyScalar(force, force, this._weightInterpose);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }

        if (this.on(BehaviorType.hide)) {
            force = this.hide(this._targetAgent1, this._vehicle.world.obstacles);
            cc.Vec2.multiplyScalar(force, force, this._weightHide);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }

        if (this.on(BehaviorType.followPath)) {
            force = this.followPath();
            cc.Vec2.multiplyScalar(force, force, this._weightFollowPath);
            if (!this.accumulateForce(force)) return this._steeringForce;
        }

        return this._steeringForce;
    }
    private calculateDithered(): cc.Vec2 { return cc.Vec2.ZERO }

    private getHidingPosition(posOb: cc.Vec2, radiusOb: number, posHunter: cc.Vec2): cc.Vec2 { return cc.Vec2.ZERO }

    public constructor(agent: Vehicle) {
        let theta = 2 * Math.random() * Math.PI;
        cc.Vec2.set(this._wanderTarget, this._wanderRadius * Math.cos(theta), this.wanderRadius * Math.sin(theta));
        this._vehicle = agent;
        this._path = new Path();
        let rect = this._vehicle.world.rect;
        this._path.createRandomPath(Math.round(Math.random() * 7 + 4), rect.xMin, rect.yMin, rect.xMax, rect.yMax, true)
    }

    public calculate(): cc.Vec2 {
        this._steeringForce.set(cc.Vec2.ZERO);
        if (!this.isSpacePartitioningOn()) {
            if (this.on(BehaviorType.separation) || this.on(BehaviorType.allignment) || this.on(BehaviorType.cohesion)) {
                this._vehicle.world.tagVehiclesWithinViewRange(this._vehicle, this._viewDistance);
            }
        } else {
            if (this.on(BehaviorType.separation) || this.on(BehaviorType.allignment) || this.on(BehaviorType.cohesion)) {
                //TODO: 空间划分标记，快速划分，测试样例暂不提供
            }
        }
        if (this._summingMethod == SummingMethod.prioritized) {
            this.calculatePrioritized();
        } else if (this._summingMethod == SummingMethod.weighted_average) {
            this.calculateWeightedSum();
        } else if (this._summingMethod == SummingMethod.dithered) {
            this.calculateDithered();
        }
        this.debugRender();
        return this._steeringForce;
    }

    public setTarget(t: cc.Vec2) { this._curTarget = t; }
    public setTargetAgent1(agent: Vehicle) { this._targetAgent1 = agent; }
    public setTargetAgent2(agent: Vehicle) { this._targetAgent2 = agent; }
    public get targetPos() { return this._curTarget; }
    public get targetAgent1() { return this._targetAgent1; }
    public get targetAgent2() { return this._targetAgent2; }

    public set offset(offset: cc.Vec2) { this._offset = offset }
    public get offset() { return this._offset }

    public get force() { return this._steeringForce; }

    public toggleSpacePartitioningOnOff() { this._cellSpaceOn = !this._cellSpaceOn; }
    public isSpacePartitioningOn(): boolean { return this._cellSpaceOn; }
    public setSummingMethod(sm: SummingMethod) { this._summingMethod = sm; }


    get boxLength() { return this._boxLength; }
    get wanderJitter() { return this._wanderJitter; }
    get wanderRadius() { return this._wanderRadius; }
    get separationWeight() { return this._weightSeparation; }
    get alignmentWeight() { return this._weightAlignment; }
    get cohesionWeight() { return this._weightCohesion; }

    fleeOn() { this._flags |= BehaviorType.flee; }
    seekOn() { this._flags |= BehaviorType.seek; }
    arriveOn() { this._flags |= BehaviorType.arrive; }
    wanderOn() { this._flags |= BehaviorType.wander; }
    pursuitOn(v: Vehicle) { this._flags |= BehaviorType.pursuit; this._targetAgent1 = v; }
    evadeOn(v: Vehicle) { this._flags |= BehaviorType.evade; this._targetAgent1 = v; }
    cohesionOn() { this._flags |= BehaviorType.cohesion; }
    separationOn() { this._flags |= BehaviorType.separation; }
    alignmentOn() { this._flags |= BehaviorType.allignment; }
    obstacleAvoidanceOn() { this._flags |= BehaviorType.obstacleAvoidance; }
    wallAvoidanceOn() { this._flags |= BehaviorType.wallAvoidance; }
    followPathOn() { this._flags |= BehaviorType.followPath; }
    interposeOn(v1: Vehicle, v2: Vehicle) { this._flags |= BehaviorType.interpose; this._targetAgent1 = v1; this._targetAgent2 = v2; }
    hideOn(v: Vehicle) { this._flags |= BehaviorType.hide; this._targetAgent1 = v; }
    offsetPursuitOn(v1: Vehicle, offset: cc.Vec2) { this._flags |= BehaviorType.offsetPursuit; this._offset = offset; this._targetAgent1 = v1; }
    flockingOn() { this.cohesionOn(); this.alignmentOn(); this.separationOn(); }
    flowOn() { this._flags |= BehaviorType.flow; }

    fleeOff() { if (this.on(BehaviorType.flee)) this._flags ^= BehaviorType.flee; }
    seekOff() { if (this.on(BehaviorType.seek)) this._flags ^= BehaviorType.seek; }
    arriveOff() { if (this.on(BehaviorType.arrive)) this._flags ^= BehaviorType.arrive; }
    wanderOff() { if (this.on(BehaviorType.wander)) this._flags ^= BehaviorType.wander; }
    pursuitOff() { if (this.on(BehaviorType.pursuit)) this._flags ^= BehaviorType.pursuit; }
    evadeOff() { if (this.on(BehaviorType.evade)) this._flags ^= BehaviorType.evade; }
    cohesionOff() { if (this.on(BehaviorType.cohesion)) this._flags ^= BehaviorType.cohesion; }
    separationOff() { if (this.on(BehaviorType.separation)) this._flags ^= BehaviorType.separation; }
    alignmentOff() { if (this.on(BehaviorType.allignment)) this._flags ^= BehaviorType.allignment; }
    obstacleAvoidanceOff() { if (this.on(BehaviorType.obstacleAvoidance)) this._flags ^= BehaviorType.obstacleAvoidance; }
    wallAvoidanceOff() { if (this.on(BehaviorType.wallAvoidance)) this._flags ^= BehaviorType.wallAvoidance; }
    followPathOff() { if (this.on(BehaviorType.followPath)) this._flags ^= BehaviorType.followPath; }
    interposeOff() { if (this.on(BehaviorType.interpose)) this._flags ^= BehaviorType.interpose; }
    hideOff() { if (this.on(BehaviorType.hide)) this._flags ^= BehaviorType.hide; }
    offsetPursuitOff() { if (this.on(BehaviorType.offsetPursuit)) this._flags ^= BehaviorType.offsetPursuit; }
    flockingOff() { this.cohesionOff(); this.alignmentOff(); this.separationOff(); }
    flowOff() { if (this.on(BehaviorType.flow)) this._flags ^= BehaviorType.flow; }

    isFleeOn() { return this.on(BehaviorType.flee); }
    isSeekOn() { return this.on(BehaviorType.seek); }
    isArriveOn() { return this.on(BehaviorType.arrive); }
    isWanderOn() { return this.on(BehaviorType.wander); }
    isPursuitOn() { return this.on(BehaviorType.pursuit); }
    isEvadeOn() { return this.on(BehaviorType.evade); }
    isCohesionOn() { return this.on(BehaviorType.cohesion); }
    isSeparationOn() { return this.on(BehaviorType.separation); }
    isAlignmentOn() { return this.on(BehaviorType.allignment); }
    isObstacleAvoidanceOn() { return this.on(BehaviorType.obstacleAvoidance); }
    isWallAvoidanceOn() { return this.on(BehaviorType.wallAvoidance); }
    isFollowPathOn() { return this.on(BehaviorType.followPath); }
    isInterposeOn() { return this.on(BehaviorType.interpose); }
    isHideOn() { return this.on(BehaviorType.hide); }
    isOffsetPursuitOn() { return this.on(BehaviorType.offsetPursuit); }
    isFlowOn() { return this.on(BehaviorType.flow) }
    resetFlag() {
        this._flags = 0;
    }

    private _lineIntersection2D(A: cc.Vec2, B: cc.Vec2, C: cc.Vec2, D: cc.Vec2, point: cc.Vec2): [boolean, number] {
        let rTop = (A.y - C.y) * (D.x - C.x) - (A.x - C.x) * (D.y - C.y);
        let rBot = (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x);

        let sTop = (A.y - C.y) * (B.x - A.x) - (A.x - C.x) * (B.y - A.y);
        let sBot = (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x);

        if ((rBot == 0) || (sBot == 0)) {
            //lines are parallel
            return [false, 0];
        }
        let r = rTop / rBot;
        let s = sTop / sBot;
        if ((r > 0) && (r < 1) && (s > 0) && (s < 1)) {
            point.x = (B.x - A.x) * r + A.x;
            point.y = (B.y - A.y) * r + A.y;
            return [true, cc.Vec2.distance(A, B) * r];
        }
        else {
            return [false, 0];
        }
    }

    private debugRender() {
        if (this._vehicle.world.isDebug && this.debugPen) {
            this.debugPen.strokeColor = cc.Color.WHITE;
            if (this.isFollowPathOn()) {
                let path = this._path.getPath();
                for (let i = 0, len = path.length; i < len; i++) {
                    let pos = path[i];
                    if (i == 0) {
                        this.debugPen.moveTo(pos.x, pos.y);
                    } else {
                        this.debugPen.lineTo(pos.x, pos.y)
                    }
                }
                this.debugPen.stroke();
            }
            if (this.isWanderOn()) {
                this._vehicle.node.getLocalMatrix(tempMat4);
                cc.Vec2.set(tempVec2_2, 0, this._wanderDistance * this._vehicle.radius);
                cc.Vec2.transformMat4(tempVec2_2, tempVec2_2, tempMat4);
                this.debugPen.strokeColor = cc.Color.GREEN;
                this.debugPen.circle(tempVec2_2.x, tempVec2_2.y, this._wanderRadius * this._vehicle.radius);
                this.debugPen.stroke();
                this.debugPen.strokeColor = cc.Color.RED;
                cc.Vec2.set(tempVec2_2, 0, this._wanderDistance);
                cc.Vec2.add(tempVec2_2, this._wanderTarget, tempVec2_2);
                cc.Vec2.multiplyScalar(tempVec2_2, tempVec2_2, this._vehicle.radius);
                cc.Vec2.transformMat4(tempVec2_2, tempVec2_2, tempMat4);
                this.debugPen.circle(tempVec2_2.x, tempVec2_2.y, 3);
                this.debugPen.stroke();
            }
            if(this.isPursuitOn()||this.isSeekOn()){
                this.debugPen.fillColor =cc.Color.RED;
                this.debugPen.circle(this._seekVec.x, this._seekVec.y, 10);
                this.debugPen.fill();
            }
        }
    }
}